/*
 *  NMEA.c
 *
 *  Created on: 2020年3月14日
 *      Author: Administrator
 *		Node  :	
 */

/* 包含头文件 ------------------------------------------------------------------*/
#include <NMEA.h>
#include <stdbool.h>
#include <math.h>
/* 私有宏定义 ------------------------------------------------------------------*/

/* 私有类型定义 -----------------------------------------------------------------*/

/* 扩展变量 -------------------------------------------------------------------*/
NMEA_STRU gps;

/* 私有变量 -------------------------------------------------------------------*/
const double a = 6378245.0;
const double ee = 0.00669342162296594323;
const double pi = 3.14159265358979324;
/* 私有函数原形 ----------------------------------------------------------------*/
static bool outOfChina(double lat, double lon);
static double transformLat(double x, double y);
static double transformLon(double x, double y);
/* 函数体 --------------------------------------------------------------------*/

void NMEA_Transform(double wgLat, double wgLon, double *mgLat, double *mgLon)
{
	if (outOfChina(wgLat, wgLon))
	{
		*mgLat = wgLat;
		*mgLon = wgLon;
		return;
	}
	double dLat = transformLat(wgLon - 105.0, wgLat - 35.0);
	double dLon = transformLon(wgLon - 105.0, wgLat - 35.0);
	double radLat = wgLat / 180.0 * pi;
	double magic = sin(radLat);
	magic = 1 - ee * magic * magic;
	double sqrtMagic = sqrt(magic);
	dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
	dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
	*mgLat = wgLat + dLat;
	*mgLon = wgLon + dLon;
}

/**
 * @brief	WGS_84 坐标系统转 GCJ_02 坐标系统
 * @param
 * @return
 * @note	OpenLuat 的所有 GNSS 模块 (立即购买) 均使用国际标准 WGS-84 坐标系，所以开发者在国内常见地图定位时，会发现与实际情况有几十米的误差。
 * 				这并非模块问题， 而是国内地图采用了非标坐标系所致。
 *				国内常见地图如高德地图使用 GCJ-02 坐标系， 百度地图使用 BD-09 坐标系，故此开发者需要对模块输出的经纬度进行加偏处理，才能在国内的地图上实现精确定位。
 */
void NMEA_WGS_84toGCJ_02(double wgLat, double wgLon, double *mgLat, double *mgLon)
{
	char latstr[20] = { 0 }, lngstr[20] = { 0 };
	sprintf(latstr, "%lf", wgLat);
	sprintf(lngstr, "%lf", wgLon);

	char lat[15], lng[15];
	ddmm2dd((char *) latstr, (char*) lat);
	ddmm2dd((char *) lngstr, (char*) lng);
	double dlat, dlng;
	sscanf(lat, "%lf", &dlat);
	sscanf(lng, "%lf", &dlng);

	*mgLat = dlat;
	*mgLon = dlng;
	
//	double rlat, rlng;
//	NMEA_Transform(dlat, dlng, mgLat, mgLon);
}

//
// Krasovsky 1940
//
// a = 6378245.0, 1/f = 298.3
// b = a * (1 - f)
// ee = (a^2 - b^2) / a^2;
static bool outOfChina(double lat, double lon)
{
	if (lon < 72.004 || lon > 137.8347)
		return true;
	if (lat < 0.8293 || lat > 55.8271)
		return true;
	return false;
}

static double transformLat(double x, double y)
{
	double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
	ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
	ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
	ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;
	return ret;
}

static double transformLon(double x, double y)
{
	double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
	ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
	ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
	ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;
	return ret;
}

/*
 * 	 转换规则:ddmm.mmmm × dd.dddd
 * 1 ddmm.mmmm / 100
 * 2 提取出 dd + (mmmmmm / 60)[舍弃掉小数位]
 * 3 结果:dd.mmmmmm
 * */
int ddmm2dd(char *ddmm, char *dd)
{
	char tstr[11];
	strcpy((char*) tstr, ddmm);

	int i = 2;
	while(i--)
	{
		char *dot = strchr(tstr, '.');
		char *pre = dot - 1;
		*dot = *pre;
		*pre = '.';
	}

	int tval;
	sscanf(tstr, "%*[^.].%d", &tval);
	tval /= 60;

	char substr[10];
	sscanf(tstr, "%[^.]", substr);

	memset(tstr, 0, strlen(tstr));
	sprintf(tstr, "%s.%d", substr, tval);

	strcpy(dd, tstr);
}

//{[]} 命令返回字符床编码
u8 checkSum(u8 * str_buf)
{
	int i, result;

	for (result = str_buf[1], i = 2; str_buf[i] != '*'; i++)
	{
		result ^= str_buf[i];
	}
//	printf("%x", result);
	return result;
}

//@ 解析基于 MVNE 协议的定位数据
u8 NMEA_AnalysisGPSData(u8 * buffer, NMEA_STRU * gps)
{
	static char GNVTG_str[100] = { 0 };
	static char GNGGA_str[100] = { 0 };
	static char GNRMC_str[100] = { 0 };
	static char GPGSA_str[100] = { 0 };
	static char BDGSA_str[100] = { 0 };
	static char GPGSV_str[100] = { 0 };
	static char BDGSV_str[100] = { 0 };
//	static char GNGLL_str[100] = { 0 };

	GGA_STRU * gga = &gps->gga;
	GLL_STRU * gll = &gps->gll;
	GSA_STRU * gsa = &gps->gsa;
	GSV_STRU * gsv = &gps->gsv;
	RMC_STRU * rmc = &gps->rmc;
	VTG_STR * vtg = &gps->vtg;
	
	
	sscanf((char *) buffer, "%[^\r]\n%[^\r]\n%[^\r]\n%[^\r]\n%[^\r]\n%[^\r]\n%[^\r]\n", 
			GNGGA_str, GPGSA_str, BDGSA_str, GPGSV_str, BDGSV_str,GNRMC_str, GNVTG_str);

	memset(gps, 0, sizeof(NMEA_STRU));

	char * formart_str = "$GNRMC,%lf,%c,%lf,%c,%lf,%c,%lf,%lf,%d,,,%c*%x";
	sscanf((char *) GNRMC_str, formart_str, &rmc->utc_time, &rmc->data_valid, &rmc->latitude, &rmc->n_s,
			&rmc->longitude, &rmc->e_w, &rmc->speed, &rmc->cog, &rmc->date, &rmc->positioning_mode, &rmc->check_sum);

	//{if} 数据无效直接返回
	if (rmc->data_valid == 'V')
		return 1;
	//{if} chenck sum 错误直接返回
	if (checkSum((u8 *) GNRMC_str) != rmc->check_sum)
		return 2;

	//{if} 获取不到需要的数据，异常返回
	if ((rmc->latitude || rmc->n_s || rmc->longitude || rmc->e_w || rmc->utc_time || rmc->speed || rmc->cog || rmc->date)
			== 0)
		return 3;

//	char print_str[256];
//	char * sformart_str = "UTC 时间=%lf,数据有效性=%c,纬度=%lf,南北极=%c,经度=%lf,西北极=%c,对地速度=%lf,对地方向=%lf,时间=%d,配置模式=%c,校验和=%d";
//	sprintf(print_str,sformart_str,
//		rmc->utc_time,
//		rmc->data_valid,
//		rmc->latitude,
//		rmc->n_s,
//		rmc->longitude,
//		rmc->e_w,
//		rmc->speed,
//		rmc->cog,
//		rmc->date,
//		rmc->positioning_mode,
//		rmc->check_sum);
//	printf("%s \r\n",print_str);

//	formart_str = "$GNGGA,%lf,%lf,%c,%lf,%c,%d,%d,%lf,%lf,%c,%lf,%c,,*%x";
//	sscanf((char *) GNGGA_str, formart_str, &gga->utc_time, &gga->latitude, &gga->n_s, &gga->longitude, &gga->e_w,
//			&gga->fix_status, &gga->number_of_sv, &gga->hdop, &gga->altitude, &gga->altitude_unit,
//			&gga->geoid_separation, &gga->geoid_separation_unit, &gga->check_sum);

//	//{if} chenck sum 错误直接返回
//	if (checkSum((u8 *) GNGGA_str) != gga->check_sum)
//		return 4;

//	//{if} 获取不到需要的数据，异常返回
//	if ((gga->utc_time || gga->latitude || gga->n_s || gga->longitude || gga->e_w || gga->fix_status
//			|| gga->number_of_sv || gga->hdop || gga->altitude || gga->altitude_unit || gga->geoid_separation
//			|| gga->geoid_separation_unit) == 0)
//		return 5;

//	sformart_str = "UTC 时间=%lf,纬度=%lf,N/S 指示=%c,经度=%lf,E/W 指示=%c,fix_status=%d,卫星数量=%d,水平精度=%lf,高地=%lf,高地单位=%c,海拔高度=%lf,海拔高度单位=%c,*校验和=%d";
//	sprintf(print_str,sformart_str,
//	gga->utc_time,	
//	gga->latitude,	
//	gga->n_s,					
//	gga->longitude,		
//	gga->e_w,					
//	gga->fix_status,	
//	gga->number_of_sv,
//	gga->hdop,				
//	gga->altitude, 	
//	gga->altitude_unit,	
//	gga->geoid_separation,
//	gga->geoid_separation_unit,
//	gga->check_sum);
//	printf("%s \r\n",print_str);

//	formart_str = "$GNVTG,%lf,%c,,%c,%lf,%c,%lf,%c,%c*%x";
//	sscanf((char *) GNVTG_str, formart_str, &vtg->cog_t, &vtg->t, &vtg->m, &vtg->speed_knots, &vtg->n, &vtg->speed_km,
//			&vtg->k, &vtg->positioning_mode, &vtg->check_sum);

//	//{if} chenck sum 错误直接返回
//	if (checkSum((u8 *) GNVTG_str) != vtg->check_sum)
//		return 6;

//	//{if} 获取不到需要的数据，异常返回
//	if ((vtg->cog_t || vtg->t || vtg->m || vtg->speed_knots || vtg->n || vtg->speed_km || vtg->k
//			|| vtg->positioning_mode || vtg->check_sum) == 0)
//		return 7;

//	sformart_str = "实际对地方向=%lf,参考方向=%c,磁极=%c,对地速度=%lf,单位=%c,对地速度,km=%lf,单位=%c,模式=%c*校验和=%d";
//	sprintf(print_str,sformart_str,
//		vtg->cog_t,
//		vtg->t,
//		vtg->m,
//		vtg->speed_knots,
//		vtg->n,
//		vtg->speed_km,
//		vtg->k,
//		vtg->positioning_mode,
//		vtg->check_sum);
//	printf("%s \r\n",print_str);

	return 0;
}

